#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"
#include "face_interfaces/srv/partol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

using SetParameters=rcl_interfaces::msg::SetParametersResult;
using namespace std::chrono_literals;

class TurtirCircleNode :public rclcpp::Node
{
    private:
        OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
        rclcpp::Service<face_interfaces::srv::Partol>::SharedPtr service_;
        // 发布者智能指针
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
        // 订阅者智能指针
        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber_; 

        double target_x_{1.0};
        double target_y_{1.0};
        double k_{1.0};//比例系数
        double speed_max_{3.0};//最大速度


    public:
        // explicit防止隐式转换
        explicit TurtirCircleNode(const std::string& node_name ):Node(node_name)
        {
            // 声明获取参数
            this->declare_parameter("k",1.0);
            this->declare_parameter("speed_max",3.0);
            this->declare_parameter("target_x",1.0);
            this->declare_parameter("target_y",1.0);
            this->get_parameter("k",k_);
            this->get_parameter("speed_max",speed_max_);
            this->get_parameter("target_x",target_x_);
            this->get_parameter("target_y",target_y_);
            // 参数实施变化
            parameters_callback_handle_=this->add_on_set_parameters_callback([&](const std::vector<rclcpp::Parameter> & parameters)->rcl_interfaces::msg::SetParametersResult{
                rcl_interfaces::msg::SetParametersResult result;
                result.successful = true;
                for (const auto & parameter : parameters) {
                    // if (!some_condition) {
                    // result.successful = false;
                    // result.reason = "the reason it could not be allowed";
                    // }
                    RCLCPP_INFO(this->get_logger(),"更新参数%s=%f",parameter.get_name().c_str(),parameter.as_double());
                    if (parameter.get_name()=="k")
                    {
                        k_=parameter.as_double();
                    }
                    if (parameter.get_name()=="speed_max")
                    {
                        speed_max_=parameter.as_double();
                    }
                    if (parameter.get_name()=="target_x")
                    {
                        target_x_=parameter.as_double();
                    }
                    if (parameter.get_name()=="target_y")
                    {
                        target_y_=parameter.as_double();
                    }
                }
                return result;
            });
            service_=this->create_service<face_interfaces::srv::Partol>("partol",[&](
                const face_interfaces::srv::Partol::Request::SharedPtr request,face_interfaces::srv::Partol::Response::SharedPtr response)->void{
                   if(
                    (0<request->target_x && request->target_x <12.0f)&&
                    (0<request->target_y && request->target_y <12.0f))
                        {
                        this->target_x_=request->target_x;
                        this->target_y_=request->target_y;
                        response->result=face_interfaces::srv::Partol::Response::SUCESS;

                        }else
                        {
                            response->result=face_interfaces::srv::Partol::Response::FAIL;
                        }

                    });
            // 创建发布者
            publisher_=this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
            
            subscriber_=this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&TurtirCircleNode::sub_callback,
            this,std::placeholders::_1));
            
        }
    
        void sub_callback(const turtlesim::msg::Pose::SharedPtr pose)
        {
            // 获取当前位置
            auto x=pose->x;
            auto y=pose->y;
            // RCLCPP_INFO(get_logger(),"当前x=%f，y=%f",x,y);

            // 计算海龟和目标的差距

            auto ditance=std::sqrt(
                (target_x_-x)*(target_x_-x)+
                (target_y_-y)*(target_y_-y)
            );
            auto angle=std::atan2((target_y_-y),(target_x_-x))-pose->theta;

            // 控制策略
            auto msg =geometry_msgs::msg::Twist();
            if(ditance>0.1){
                if(fabs(angle)>0.2){
                    msg.angular.z=fabs(angle);
                }else{
                    msg.linear.x=k_*ditance;
                }
            }
            
            if (msg.linear.x> speed_max_){
                msg.linear.x= speed_max_;
            }
            publisher_->publish(msg);
        }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc,argv);
    auto node =std::make_shared<TurtirCircleNode> ("TurtirControl");
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}